#pragma config(UART_Usage, UART2, VEX_2x16_LCD, baudRate19200, IOPins, None, None)
#pragma config(Sensor, dgtl5,  solenoidH,      sensorDigitalOut)
#pragma config(Sensor, dgtl6,  solenoidBrake,  sensorDigitalOut)
#pragma config(Sensor, dgtl7,  rearTouch,      sensorTouch)
#pragma config(Sensor, dgtl8,  AutonLow,       sensorTouch)
#pragma config(Sensor, dgtl9,  AutonHigh,      sensorTouch)
#pragma config(Sensor, dgtl10, solenoidB,      sensorDigitalOut)
#pragma config(Sensor, dgtl11, solenoidL,      sensorDigitalOut)
#pragma config(Sensor, dgtl12, solenoidR,      sensorDigitalOut)
#pragma config(Motor,  port1,           collectorL,    tmotorServoContinuousRotation, openLoop, reversed, encoder, encoderPort, None, 1000)
#pragma config(Motor,  port2,           frontL,        tmotorServoContinuousRotation, openLoop)
#pragma config(Motor,  port3,           frontL2,       tmotorServoContinuousRotation, openLoop)
#pragma config(Motor,  port4,           backL,         tmotorServoContinuousRotation, openLoop)
#pragma config(Motor,  port5,           backL2,        tmotorServoContinuousRotation, openLoop)
#pragma config(Motor,  port6,           frontR,        tmotorServoContinuousRotation, openLoop)
#pragma config(Motor,  port7,           frontR2,       tmotorServoContinuousRotation, openLoop)
#pragma config(Motor,  port8,           backR,         tmotorServoContinuousRotation, openLoop)
#pragma config(Motor,  port9,           backR2,        tmotorServoContinuousRotation, openLoop)
#pragma config(Motor,  port10,          collectorR,    tmotorServoContinuousRotation, openLoop, encoder, encoderPort, None, 1000)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

#pragma platform(VEX)
#pragma competitionControl(Competition)
#pragma autonomousDuration(20)
#pragma userControlDuration(120)

#include "Vex_Competition_Includes.c"
#include "Zeta Function Library Worlds.c"
#include "Usercontrol Worlds.c"
#include "Autonomous Worlds.c"

bool selecting = true;
bool skills = false;
int zone = 0;         // INT = 0, ISO  = 1
int autonNumber = 0;

void pre_auton(){
  bStopTasksBetweenModes = true;
  SensorValue[solenoidB] = 1;
  bLCDBacklight = true;
  displayLCDPos(0,0);
  while(selecting == true){
    clearLCDLine(0);
    displayLCDPos(0,0);
    displayNextLCDString("RST  MATCH SKILLS");
    while(nLCDButtons != 1 && nLCDButtons != 2 && nLCDButtons != 4);
    if (nLCDButtons == 2){
      while(nLCDButtons != 0);
	    clearLCDLine(0);
	    displayLCDPos(0,0);
	    displayNextLCDString("RST   INT    ISO");
	    while(nLCDButtons != 1 && nLCDButtons != 2 && nLCDButtons != 4);
	    if (nLCDButtons == 2){
	      while(nLCDButtons != 0);
	      zone = 0;
	    } else if(nLCDButtons == 4){
	      while(nLCDButtons != 0);
	      zone = 1;
	    } else if(nLCDButtons == 1){
	      while(nLCDButtons != 0);
	      continue;
	    }
	    clearLCDLine(0);
	    displayLCDPos(0,0);
	    displayNextLCDString("RST   ONE    TWO");
	    while(nLCDButtons != 1 && nLCDButtons != 2 && nLCDButtons != 4);
	    if (nLCDButtons == 2) {
	      while(nLCDButtons != 0);
	      autonNumber = 0;
	    } else if (nLCDButtons == 4) {
	      while(nLCDButtons != 0);
	      autonNumber = 1;
	    } else if (nLCDButtons == 1) {
	      while(nLCDButtons != 0);
	      continue;
	    }
    } // END if(nLCDbuttons == 2);
    else if(nLCDButtons == 4){
      while(nLCDButtons != 0);
      skills = true;
    } else if(nLCDButtons == 1){
      while(nLCDButtons != 0);
      continue;
    }
    selecting = false; ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
  }
	  if(skills == true) {
	    clearLCDLine(0);
	    displayLCDPos(0,0);
	    displayNextLCDString("    SKILLS!     ");
	    wait1Msec(2000);
	    bLCDBacklight = false;
	  } else {
	  if(zone == 0 && autonNumber == 0) {
	    clearLCDLine(0);
	    displayLCDPos(0,0);
	    displayNextLCDString("INTERACTION No 1");
	    wait1Msec(2000);
	    bLCDBacklight = false;
	  }
	  if(zone == 0 && autonNumber == 1) {
	    clearLCDLine(0);
	    displayLCDPos(0,0);
	    displayNextLCDString("INTERACTION No 2");
	    wait1Msec(2000);
	    bLCDBacklight = false;
	  }
	  if(zone == 1 && autonNumber == 0) {
	    clearLCDLine(0);
	    displayLCDPos(0,0);
	    displayNextLCDString("ISOLATION No 1");
	    wait1Msec(2000);
	    bLCDBacklight = false;
	  }
	  if(zone == 1 && autonNumber == 1) {
	    clearLCDLine(0);
	    displayLCDPos(0,0);
	    displayNextLCDString("ISOLATION No 2");
	    wait1Msec(2000);
	    bLCDBacklight = false;
	  }
	}
}//end pre_auton

task autonomous(){
  if(skills){
    skillsAutonomous();
  }else{
	  if(zone == 0 && autonNumber == 0){
	      interactionAuton1();
	  }
	  if(zone == 0 && autonNumber == 1){
	      interactionAuton2();
	  }
	  if(zone == 1 && autonNumber == 0){
	      isolationAuton1();
	  }
	  if(zone == 1 && autonNumber == 1){
	      isolationAuton2();
	  }
	}// END else
}// END autonomous

task usercontrol()
{
 while (true)
 {
		drivetrain();
		collector();
		lift();
		block();
		diagonaltrigger();
		twotrigger();
		fourtrigger();
		brake();
 }//end while loop
}//end task usercontrol
